#include "device_function.h"
#include "zf_common_headfile.h"
#include "device_imageProcess.h"

#define VALID_HEIGHT 20
/*
功能：中线误差
*/
/*
功能：计算中线偏差
*/
int Err_Sum()
{
    int i=0,sum=0;
    for(i=image_h-1; i>image_h-VALID_HEIGHT-1 && i>=0; i--)
    {
        sum+=(image_w/2)-(center_line[i]);
    }
    return sum;
}

/*
功能： 舵机PWM限幅函数,避免卡到最大值
*/
uint16_t limit_servo_pwm(uint16_t pwm,uint16_t pwm_Mix,uint16_t pwm_Max)
{
    if(pwm < pwm_Mix) return pwm_Mix;
    if(pwm > pwm_Max) return pwm_Max;
    return pwm;
}

/*
功能：位置式PID--舵机PD控制器
*/
int16 PD_Camera(float expect_val, float err,float P_argv,float D_argv)
{
    static float last_output = 0;  // 上一次的输出值
    float u;
    float P = P_argv;  // 比例系数
    float D = D_argv;  // 微分系数
    volatile static float error_current, error_last;
    float ek, ek1;
    
    // 计算误差
    error_current = err - expect_val;
    ek = error_current;
    ek1 = error_current - error_last;
    
    // PD控制
    u = P * ek + D * ek1;

    last_output = u;
    
    error_last = error_current;

    return u;
}
/****************位置式PID-->end****************/


/*******************控制舵机部分-->start*********************/
int16 errServo = 0; //舵机误差
int16 Steer_PWM = 0;//通过PD控制设置舵机的PWM
float P_argv = 0, D_argv = 0;
extern uint16 target_servo;
carCruiseTask myCarCruiseTask = offCruiseState;//默认正常执行状态
void servoDirectionControl(carCruiseTask myCarCruiseTask)
{
    errServo = Err_Sum();
    switch (myCarCruiseTask)
    {
    case offCruiseState:break;//关闭默认巡航
    case onCruiseState:P_argv = 1.5;D_argv = 8.5;break;//打开默认巡航
    case acceleratedCruiseState:P_argv = 0.55;D_argv = 10.5;break;//加速巡航
    }
    Steer_PWM = PD_Camera(0, errServo, P_argv, D_argv);
    target_servo = limit_servo_pwm(STEER_MID+Steer_PWM,STEER_RIGHT,STEER_LEFT);
}
/*******************控制舵机部分-->end*********************/

/*******************出库部分-->start*********************/
uint8 outBoundFinishedFlag = false;//出库完成标志位（false：未完成，true：完成）
outBoundTaskState myOutBoundTaskState = goStraight;
uint8 outBoundcounter = 0; //出库任务计数
extern int16 target_speed;
extern int16 current_target_speed;

/*
出库
*/
void outBoundfunction()
{
    if(!outBoundFinishedFlag) 
    {
        switch(myOutBoundTaskState)
        {
            case goStraight:
                if(outBoundcounter<15)
                {
                    target_servo = STEER_MID;
                    target_speed = 100;
                    outBoundcounter++;
                   
                }else{
                    printf("出库直行\n");
                    myOutBoundTaskState = rightTurn;
                    outBoundcounter = 0;
                }
            break;
            case rightTurn:
            
                if(outBoundcounter<30)
                {
                    target_servo = STEER_RIGHT;
                    target_speed = 100;
                    outBoundcounter++;
                }else{
                    printf("出库右拐\n");
                    target_speed = 100;
                    mycarTask = followLineState;
                    outBoundFinishedFlag = true;
                    outBoundcounter = 0;
                }
            break;
        }
    }
}


/*侧方停车*/
//侧方停车开始标志位
bool sideParkingFinishedFlag = false;
sideParkingTask mysideParkingTask = PARKING_READY;//默认靠边
uint8 sideParkingCounter = 0;

void sideParkingFunction()
{
    if(!sideParkingFinishedFlag) 
    {
        switch(mysideParkingTask)
        {
            case PARKING_READY: //先直行一段距离
                if(sideParkingCounter<35)
                {
                    sideParkingCounter++;
                    target_servo = STEER_MID;
                    target_speed = 50;
                }else{
                    printf("正在靠边...\n");
                    mysideParkingTask = PARKING_REVERSE_RIGHT;
                    sideParkingCounter = 0;
                }
            break;
            case PARKING_REVERSE_RIGHT: //右打死倒退
                 
                if(sideParkingCounter<68)
                {
                    target_servo = STEER_RIGHT;
                    sideParkingCounter++;
                    target_speed = -50;
                }else{
                    printf("右打死倒退...\n");
                    mysideParkingTask = PARKING_REVERSE_STRAIGHT;
                    sideParkingCounter = 0;
                }

            break;
            case PARKING_REVERSE_STRAIGHT: // 回正倒退
           
                if(sideParkingCounter<30)
                {
                    target_servo = STEER_MID;
                    sideParkingCounter++;
                    target_speed = -50;
                }else{
                    printf("回正倒退...\n");
                    mysideParkingTask = PARKING_REVERSE_LEFT;
                    sideParkingCounter = 0;
                }
                
            break;
            case  PARKING_REVERSE_LEFT: //左打死倒退
             
                if(sideParkingCounter<40)
                {
                    target_servo = STEER_LEFT;
                    sideParkingCounter++;
                    target_speed = -50;
                }else{
                    printf("左打死倒退...\n");
                    mysideParkingTask = PARKING_FORWARD_LEFT;
                    sideParkingCounter = 0;
                }
                
            break;
            case  PARKING_FORWARD_LEFT: //左打死前进
            
                if(sideParkingCounter<65)
                {
                    target_servo = STEER_LEFT;
                    sideParkingCounter++;
                    target_speed = 50;
                }else{
                    printf("左打死前进...\n");
                    mysideParkingTask = PARKING_FORWARD_STRAIGHT;
                    sideParkingCounter = 0;
                }
            break;
            case PARKING_FORWARD_STRAIGHT: //回正前进
                
                if(sideParkingCounter<45)
                {
                    target_servo = STEER_MID;
                    sideParkingCounter++;
                    target_speed = 50;
                }else{
                    printf("回正前进...\n");
                    mysideParkingTask = PARKING_FORWARD_RIGHT;
                    sideParkingCounter = 0;
                }
                
            break;
            case PARKING_FORWARD_RIGHT: // 右打死前进
                if(sideParkingCounter<45)
                {
                    target_servo = STEER_RIGHT;
                    sideParkingCounter++;
                    target_speed = 50;
                }else{
                    printf("右打死前进...\n");
                    mysideParkingTask = PARKING_COMPLETED;
                    sideParkingCounter = 0;
                }
            break;
            case PARKING_COMPLETED:
                 printf("开始循迹!!!\n");
                 target_speed = 200;
                 isAccelerate = false;
                 mycarTask = followLineState;
                 sideParkingFinishedFlag = true;
            break;
        }
        
    }
}


/*雷达入口初始化*/
bool radarEntryFlag = false;
radarEntry myradarEntry = radarLeft;
uint8 radarCounter = 0;
void radarEntryInit(void)
{
    if(!radarEntryFlag)
    {
        switch(myradarEntry)
        {
            case radarLeft:
                 if(radarCounter < 10) {
                    target_servo = STEER_LEFT;
                    target_speed = 50;
                    radarCounter++;
                } else {
                    printf("出口正在左拐...\n");
                    myradarEntry = radarGo;
                    radarCounter = 0;
                }
            break;
            case radarGo:
                if(radarCounter<25)
                {
                    target_servo = STEER_MID;
                    radarCounter++;
                    target_speed = 100;
                }else{
                    printf("出口正在完成...\n");
                    myradarEntry = radarStart;
                    radarCounter = 0;
                }          
            break;
            case radarStart:
                 target_speed = 0;
                 printf("停止...\n");
                radarEntryFlag = true;
            break;
            
        }
    }
    printf("radarCounter:%d\n",radarCounter);
}



/*******************出库部分-->end*********************/
carTask mycarTask = outBoundState;
//检测停车区域函数(检测一块区域)
#define leftValueMax 14
#define leftValueMin 7

#define rightValue 156
#define arrflag 116 //第110行
int inspectGarageState = 0;//检测是否存在车库
int startFlag = -1;
int inspectOutState = 0;//检测出口


//侧方停车检测区域
#define inspectMix 116
#define inspectMax 116
int stopCarCounter = 0;//记录检测到车库的次数
bool isAccelerate = false;
uint16 detectRealIntersectionCounter = 0;

extern bool radarModelFlag;
int8 stateradar = 0;
uint8 stateradarCounter = 0;
bool radarFlag = false;
// /*******************小车全局任务函数*********************/
void carOverallTask()
{
    switch(mycarTask)
    {
        case outBoundState:image_ProcessTask(false);outBoundfunction();break;//出库任务
        case followLineState:
            image_ProcessTask(true);
            switch(isAccelerate)
            {
                case false:followLineTask(onCruiseState);break;
                case true:followLineTask(acceleratedCruiseState);break;
            }
            
        break;//默认巡航任务

        case sideParkingState:image_ProcessTask(false);sideParkingFunction();break;//侧方停车
        //case radarState:image_ProcessTask(false);radarEntryInit();break;//雷达任务开启状态
        case radarState:isAccelerate = false;image_ProcessTask(false);
        
        target_speed = 50;
            switch(stateradar)
            {

                case 0:
                    if(stateradarCounter<2)
                    {
                        target_servo = STEER_MID;
                        stateradarCounter++;
                        target_speed = 50;
                    }else{
                        
                        stateradar = 1;
                        stateradarCounter = 0;
                    }          
                break;
                case 1:
                    if(stateradarCounter<30)
                    {
                        target_servo = STEER_LEFT;
                        stateradarCounter++;
                        target_speed = 50;
                    }else{
                        
                        stateradar = 2;
                        stateradarCounter = 0;
                    }          
                break;
                case 2:
                     if(stateradarCounter<15)
                    {
                        target_servo = STEER_MID;
                        stateradarCounter++;
                        target_speed = 50;
                    }else{
                        printf("识别到雷达入口\n");
                        stateradar = 3;
                        stateradarCounter = 0;
                    }         
                break;
                    
                case 3:
                    target_speed = 0;
                    target_servo = STEER_MID;
                    pit_ms_stop();
                    radarModelFlag = true;
                break;
            }
        
        
        break;//雷达任务开启状态
    }

    // //侧方停车
    // if(outBoundFinishedFlag == true)
    // {   
    //     for(int i=inspectMix;i<=inspectMax;i++)
    //     {
    //         switch(inspectGarageState)
    //         {
    //             case 0:
    //             if(l_border[i] >= leftValueMin && l_border[i] <= leftValueMax && r_border[i]>= rightValue && r_border[i]-l_border[i] >= rightValue-leftValueMax)
    //             {
    //                 if(l_border[i+1] >= leftValueMin &&l_border[i+1] <= leftValueMax && r_border[i+1]>= rightValue && r_border[i+1]-l_border[i+1] >= rightValue-leftValueMax)
    //                 {
    //                     if(l_border[i+2] >= leftValueMin &&l_border[i+2] <= leftValueMax && r_border[i+2]>= rightValue && r_border[i+2]-l_border[i+2] >= rightValue-leftValueMax && startFlag == -1)
    //                     {
    //                         startFlag = 1;
    //                         target_speed = 50;
    //                         printf("r_border[%d]-l_border[%d]=%d\n",r_border[i],l_border[i],r_border[i]-l_border[i]);
    //                         printf("r_border[%d+1]-l_border[%d+1]=%d\n",r_border[i],l_border[i],r_border[i+1]-l_border[i+1]);
    //                         printf("r_border[%d+2]-l_border[%d+2]=%d\n",r_border[i],l_border[i],r_border[i+2]-l_border[i+2]);
    //                         printf("侧方停车位置识别开始\n"); 
    //                     }
    //                 }
    //             }else if(l_border[i] > leftValueMax && r_border[i] < rightValue && r_border[i]-l_border[i] < rightValue-leftValueMax)
    //             {
    //                  if(l_border[i+1] > leftValueMax && r_border[i+1] < rightValue && r_border[i+1]-l_border[i+1] < rightValue-leftValueMax)
    //                 {
    //                     if(l_border[i+2] > leftValueMax && r_border[i+2] < rightValue && r_border[i+2]-l_border[i+2] < rightValue-leftValueMax && startFlag == 1)
    //                     {
    //                         //当i==110也不符合，说明真的结束了
    //                         if(i == inspectMax)
    //                         {
    //                             printf("侧方停车位置识别结束\n");
    //                             startFlag = 2;
    //                             inspectGarageState = 1;
    //                         }
                                
    //                     }
    //                 }
    //             }isAccelerate = false;

    //             break;
    //             case 1: mycarTask = sideParkingState;outBoundFinishedFlag = 3;break;
    //         }
            
    //     }
    // }

    //实时检测出口，同时检测，当outBoundFinishedFlag == 3时，说明已经侧方停车过，需要等待检测出口 && outBoundFinishedFlag == 3
    if(detectRealIntersection(l_border,r_border,center_line) == true && outBoundFinishedFlag == 3)
    {
        
        mycarTask = radarState;
       
    }

    //识别急转弯
    
}


